
#ifndef AFTERMATH_NUMERICS_VSSE_MATRIX_HPP_INCLUDED
#define AFTERMATH_NUMERICS_VSSE_MATRIX_HPP_INCLUDED

#include "../aligned_memory.hpp"
#include "matrix.hpp"
#include "vsse_math.hpp"

#include <immintrin.h>
#include <memory.h>
#include <mmintrin.h>
#include <smmintrin.h>
#include <utility>

namespace aftermath
{
    namespace numerics
    {
        /** AVX. */
        template <int M, int N>
        struct matrix<M, N, double, __m256d>
        {
            template <int K, int L, typename T, typename S>
            friend struct matrix;

            typedef double data_type;
            typedef __m256d sse_type;

            static const int height = M;
            static const int width = N;
            
            static const bool is_square = (M == N);
            static const int memory_wasted = (N % 4) * sizeof(data_type);
        
            static const int required_align = 32;
            static const int stride = (N + 3) / 4;
            static const int required_memory = M * stride * sizeof(sse_type);
            static const int sufficient_memory = sufficient_memory_size<required_memory, required_align>::value;

            matrix()
                : m_memory(nullptr), m_data(nullptr)
            {
                //this->m_memory = new char[sufficient_memory];
                this->m_memory = malloc(sufficient_memory);
                this->m_data = aligned_memory<sse_type, required_align>::address(this->m_memory);
                /** Clean border. */
                if (N % 4 != 0) for (int i = 0; i < M; i++) this->get_sse_element(i, N / 4) = _mm256_setzero_pd();
            }

            matrix(const matrix& other)
                : m_memory(nullptr), m_data(nullptr)
            {
                //this->m_memory = new char[sufficient_memory];
                this->m_memory = malloc(sufficient_memory);
                this->m_data = aligned_memory<sse_type, required_align>::address(this->m_memory);
                std::memcpy(this->m_data, other.m_data, required_memory); 
            }

            matrix(matrix&& other)
                : m_memory(nullptr), m_data(nullptr)
            {
                *this = std::move(other);
            }

            matrix& operator =(const matrix& other) 
            {
                if (this != &other) std::memcpy(this->m_data, other.m_data, required_memory);
                return *this;
            }

            matrix& operator =(matrix&& other)
            {
                if (this != &other)
                {
                    free(this->m_memory);
                    this->m_data = other.m_data;
                    this->m_memory = other.m_memory;
                    other.m_data = nullptr;
                    other.m_memory = nullptr;
                }
                return *this;
            }

            ~matrix()
            {
                //delete[] this->m_memory;
                free(this->m_memory);
                this->m_memory = nullptr;
                this->m_data = nullptr;
            }

            bool is_neat() const
            {
                if (N % 4 == 0) return true; 
                for (int i = 0; i < M; i++) 
                {
                    data_type border = this->get_sse_element(i, N / 4).m256d_f64;
                    for (int kk = 3; kk > (N % 4) - 1; kk--) if (border[kk] != 0.0) return false;
                }
                return true;
            }
            
            void fill(data_type value)
            {
                if (N % 4 == 0)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    for (int i = 0; i < M; i++) for (int j = 0; j < N / 4; j++) this->get_sse_element(i, j) = _mm256_load_pd(v);
                }
                if (N % 4 == 1)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, 0.0, 0.0, 0.0 };
                    for (int i = 0; i < M; i++) 
                    {
                        for (int j = 0; j < N / 4; j++) this->get_sse_element(i, j) = _mm256_load_pd(v);
                        this->get_sse_element(i, N / 4) = _mm256_load_pd(vx);
                    }
                }
                if (N % 4 == 2)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, value, 0.0, 0.0 };
                    for (int i = 0; i < M; i++) 
                    {
                        for (int j = 0; j < N / 4; j++) this->get_sse_element(i, j) = _mm256_load_pd(v);
                        this->get_sse_element(i, N / 4) = _mm256_load_pd(vx);
                    }
                }
                if (N % 4 == 3)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, value, value, 0.0 };
                    for (int i = 0; i < M; i++) 
                    {
                        for (int j = 0; j < N / 4; j++) this->get_sse_element(i, j) = _mm256_load_pd(v);
                        this->get_sse_element(i, N / 4) = _mm256_load_pd(vx);
                    }
                }
            }
            
            void fill_row(int row, data_type value) 
            {
                if (N % 4 == 0)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    for (int j = 0; j < N / 4; j++) this->get_sse_element(row, j) = _mm256_load_pd(v);
                }
                if (N % 4 == 1)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, 0.0, 0.0, 0.0 };
                    for (int j = 0; j < N / 4; j++) this->get_sse_element(row, j) = _mm256_load_pd(v);
                    this->get_sse_element(row, N / 4) = _mm256_load_pd(vx);
                }
                if (N % 4 == 2)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, value, 0.0, 0.0 };
                    for (int j = 0; j < N / 4; j++) this->get_sse_element(row, j) = _mm256_load_pd(v);
                    this->get_sse_element(row, N / 4) = _mm256_load_pd(vx);
                }
                if (N % 4 == 3)
                {
                    __declspec(align(required_align)) double v[4] = { value, value, value, value };
                    __declspec(align(required_align)) double vx[4] = { value, value, value, 0.0 };
                    for (int j = 0; j < N / 4; j++) this->get_sse_element(row, j) = _mm256_load_pd(v);
                    this->get_sse_element(row, N / 4) = _mm256_load_pd(vx);
                }
            }

            void clear() { for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(i, j) = _mm256_setzero_pd(); }

            void clear_row(int row) { for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(row, j) = _mm256_setzero_pd(); }
        
            /** Elementwise operations are slow! */
            data_type get_element(int row, int column) const { return this->get_sse_element(row, column / 4).m256d_f64[column % 4]; }

            template <int row, int column>
            void set_element(data_type value)
            {
                const int offset = (column % 4) >> 1;
                __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), offset);
                if (column % 2 == 0) x = _mm_blend_pd(_mm_load_sd(&value), x, 0x2);
                if (column % 2 == 1) x = _mm_unpacklo_pd(x, _mm_load_sd(&value));
                this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, offset);
            }

            void set_element(int row, int column, data_type value)
            {
                if (column %  4 == 0)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 0);
                    x = _mm_blend_pd(_mm_load_sd(&value), x, 0x2);
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 0);
                }
                if (column %  4 == 1)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 0);
                    x = _mm_unpacklo_pd(x, _mm_load_sd(&value));
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 0);
                }
                if (column %  4 == 2)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 1);
                    x = _mm_blend_pd(_mm_load_sd(&value), x, 0x2);
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 1);
                }
                if (column %  4 == 3)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 1);
                    x = _mm_unpacklo_pd(x, _mm_load_sd(&value));
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 1);
                }
            }
            
            template <int row, int column>
            void clear_element()
            {
                const int offset = (column % 4) >> 1;
                __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), offset);
                if (column % 2 == 0) x = _mm_blend_pd(_mm_setzero_pd(), x, 0x2);
                if (column % 2 == 1) x = _mm_unpacklo_pd(x, _mm_setzero_pd());
                this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, offset);
            }
            
            void clear_element(int row, int column)
            {
                if (column %  4 == 0)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 0);
                    x = _mm_blend_pd(_mm_setzero_pd(), x, 0x2);
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 0);
                }
                if (column %  4 == 1)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 0);
                    x = _mm_unpacklo_pd(x, _mm_setzero_pd());
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 0);
                }
                if (column %  4 == 2)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 1);
                    x = _mm_blend_pd(_mm_setzero_pd(), x, 0x2);
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 1);
                }
                if (column %  4 == 3)
                {
                    __m128d x = _mm256_extractf128_pd(this->get_sse_element(row, column / 4), 1);
                    x = _mm_unpacklo_pd(x, _mm_setzero_pd());
                    this->get_sse_element(row, column / 4) = _mm256_insertf128_pd(this->get_sse_element(row, column / 4), x, 1);
                }
            }
            
            void transpose_self();
            matrix<N, M, double, __m256d> transpose() const;

            template <int K>
            matrix<M, K, double, __m256d> multiply(const matrix<N, K, double, __m256d>& other) const;

            template <int K>
            matrix<M, K, double, __m256d> fast_multiply(const matrix<N, K, double, __m256d>& other) const;
            
            matrix<M, N, double, __m256d> operator +(const matrix<M, N, double, __m256d>& other)
            {
                matrix<M, N, double, __m256d> result;
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) result.get_sse_element(i, j) = this->get_sse_element(i, j) + other.get_sse_element(i, j);
                return result;
            }
            
            matrix<M, N, double, __m256d> operator -(const matrix<M, N, double, __m256d>& other)
            {
                matrix<M, N, double, __m256d> result;
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) result.get_sse_element(i, j) = this->get_sse_element(i, j) - other.get_sse_element(i, j);
                return result;
            }

            matrix<M, N, double, __m256d> operator *(const matrix<M, N, double, __m256d>& other)
            {
                matrix<M, N, double, __m256d> result;
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) result.get_sse_element(i, j) = this->get_sse_element(i, j) * other.get_sse_element(i, j);
                return result;
            }

            matrix<M, N, double, __m256d> operator /(const matrix<M, N, double, __m256d>& other)
            {
                matrix<M, N, double, __m256d> result;
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) result.get_sse_element(i, j) = this->get_sse_element(i, j) / other.get_sse_element(i, j);
                return result;
            }

            matrix<M, N, double, __m256d>& operator +=(const matrix<M, N, double, __m256d>& other)
            {
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(i, j) += other.get_sse_element(i, j);
                return *this;
            }

            matrix<M, N, double, __m256d>& operator -=(const matrix<M, N, double, __m256d>& other)
            {
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(i, j) -= other.get_sse_element(i, j);
                return *this;
            }

            matrix<M, N, double, __m256d>& operator *=(const matrix<M, N, double, __m256d>& other)
            {
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(i, j) *= other.get_sse_element(i, j);
                return *this;
            }

            matrix<M, N, double, __m256d>& operator /=(const matrix<M, N, double, __m256d>& other)
            {
                for (int i = 0; i < M; i++) for (int j = 0; j < (N + 3) / 4; j++) this->get_sse_element(i, j) /= other.get_sse_element(i, j);
                return *this;
            }
            

        private:
            // Row-major
            void* m_memory;
            __m256d* m_data;

            int get_sse_index(int i, int j) const { return i * stride + j; }

            __m256d& get_sse_element(int i, int j) const { return this->m_data[i * stride + j]; }
        };

        template <int M, int N>
        void matrix<M, N, double, __m256d>::transpose_self()
        {
            static_assert(M == N, "transpose_self only works on square matrices.");

            __m256d utmp3, utmp2, utmp1, utmp0;
            __m256d ltmp3, ltmp2, ltmp1, ltmp0;
            __m256d zero = _mm256_setzero_pd();
            const int nBlocks = N / 4; // Number of full 4-by-4 blocks.

            // Core of the matrix: nBlocks-by-nBlocks block matrix with 4-by-4 blocks
            // Along the main diagonal do 4-by-4 transpose
            // For the off-diagonal blocks do coupled 4-by-4 transpose
            // 0-------------------------
            // |  1----------------------
            // |  |  2-------------------
            // |  |  |  3----------------
            // |  |  |  |  ...           
            for (int i = 0; i < nBlocks; i++)
            {
                // Main diagonal: 4-by-4 transpose
                quad_row_transpose_self<4>(
                    this->get_sse_element(4 * i, i), this->get_sse_element(4 * i + 1, i), this->get_sse_element(4 * i + 2, i), this->get_sse_element(4 * i + 3, i),
                    utmp0, utmp2, zero);
            
                for (int j = i + 1; j < nBlocks; j++)
                {
                    // Off-diagonal: 4-by-4 coupled transpose
                    quad_coupled_row_transpose<4>(
                        this->get_sse_element(4 * i, j), this->get_sse_element(4 * i + 1, j), this->get_sse_element(4 * i + 2, j), this->get_sse_element(4 * i + 3, j),
                        this->get_sse_element(4 * j, i), this->get_sse_element(4 * j + 1, i), this->get_sse_element(4 * j + 2, i), this->get_sse_element(4 * j + 3, i),
                        utmp0, utmp1, utmp2, utmp3,
                        ltmp0, ltmp1, ltmp2, ltmp3, zero);
                }

                // Margins
                if (N % 4 != 0)
                    quad_coupled_row_transpose<N % 4>(
                        this->get_sse_element(4 * i, nBlocks), this->get_sse_element(4 * i + 1, nBlocks), this->get_sse_element(4 * i + 2, nBlocks), this->get_sse_element(4 * i + 3, nBlocks),
                        this->get_sse_element(4 * nBlocks, i), (N % 4) < 2 ? zero : this->get_sse_element(4 * nBlocks + 1, i), (N % 4) < 3 ? zero : this->get_sse_element(4 * nBlocks + 2, i), zero,
                        utmp0, utmp1, utmp2, utmp3,
                        ltmp0, ltmp1, ltmp2, ltmp3, zero);
            }

            // Do transpose on remaining square main diagonal block
            if (N % 4 != 0)
                quad_row_transpose_self<N % 4>(
                    this->get_sse_element(4 * nBlocks, nBlocks), (N % 4) < 2 ? zero : this->get_sse_element(4 * nBlocks + 1, nBlocks), (N % 4) < 3 ? zero : this->get_sse_element(4 * nBlocks + 2, nBlocks), zero,
                    utmp0, utmp2, zero);
        }

        template <int M, int N>
        matrix<N, M, double, __m256d> matrix<M, N, double, __m256d>::transpose() const
        {
            __m256d tmp0, tmp1, tmp2, tmp3;
            __m256d zero = _mm256_setzero_pd();
            matrix<N, M, double, __m256d> result;
            const int nRowBlocks = M / 4; // Number of full 4-by-4 blocks.
            const int nColumnBlocks = N / 4; // Number of full 4-by-4 blocks.

            // Core matrix: nRowBlocks-by-nColumnBlocks block matrix with 4-by-4 blocks
            for (int i = 0; i < nRowBlocks; i++)
            {
                for (int j = 0; j < nColumnBlocks; j++)
                {
                    quad_row_transpose<4, 4>(
                        this->get_sse_element(4 * i, j), this->get_sse_element(4 * i + 1, j), this->get_sse_element(4 * i + 2, j), this->get_sse_element(4 * i + 3, j),
                        result.get_sse_element(4 * j, i), result.get_sse_element(4 * j + 1, i), result.get_sse_element(4 * j + 2, i), result.get_sse_element(4 * j + 3, i),
                        tmp0, tmp1, tmp2, tmp3, zero);
                }
            
                // Last column
                if (N % 4 != 0)
                    quad_row_transpose<4, N % 4>(
                        this->get_sse_element(4 * i, nColumnBlocks), this->get_sse_element(4 * i + 1, nColumnBlocks), this->get_sse_element(4 * i + 2, nColumnBlocks), this->get_sse_element(4 * i + 3, nColumnBlocks),
                        result.get_sse_element(4 * nColumnBlocks, i), (N % 4) < 2 ? zero : result.get_sse_element(4 * nColumnBlocks + 1, i), (N % 4) < 3 ? zero : result.get_sse_element(4 * nColumnBlocks + 2, i), zero,
                        tmp0, tmp1, tmp2, tmp3, zero);
            }
        
            // Last row
            if (M % 4 != 0)
            {
                for (int j = 0; j < nColumnBlocks; j++)
                {
                    quad_row_transpose<M % 4, 4>(
                        this->get_sse_element(4 * nRowBlocks, j), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, j), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, j), zero,
                        result.get_sse_element(4 * j, nRowBlocks), result.get_sse_element(4 * j + 1, nRowBlocks), result.get_sse_element(4 * j + 2, nRowBlocks), result.get_sse_element(4 * j + 3, nRowBlocks),
                        tmp0, tmp1, tmp2, tmp3, zero);
                }

                // Main diagonal appendix
                if (N % 4 != 0)
                    quad_row_transpose<M % 4, N % 4>(
                        this->get_sse_element(4 * nRowBlocks, nColumnBlocks), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, nColumnBlocks), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, nColumnBlocks), zero,
                        result.get_sse_element(4 * nColumnBlocks, nRowBlocks), (N % 4) < 2 ? zero : result.get_sse_element(4 * nColumnBlocks + 1, nRowBlocks), (N % 4) < 3 ? zero : result.get_sse_element(4 * nColumnBlocks + 2, nRowBlocks), zero,
                        tmp0, tmp1, tmp2, tmp3, zero);
            }

            return result;
        }

        template <int M, int N>
        template <int K>
        matrix<M, K, double, __m256d> matrix<M, N, double, __m256d>::multiply(const matrix<N, K, double, __m256d>& other) const
        {
            __m256d tmp0, tmp1, tmp2, tmp3;
            __m256d col0, col1, col2, col3;
            __m256d zero = _mm256_setzero_pd();
            matrix<M, K, double, __m256d> result;
            const int nRowBlocks = M / 4; // Number of full 4-by-4 blocks in resulting matrix.
            const int nColumnBlocks = K / 4; // Number of full 4-by-4 blocks in resulting matrix.
            const int nAuxBlocks = N / 4; // Number of full 4-by-4 blocks in auxiliary matrix.

            // Core matrix: nRowBlocks-by-nColumnBlocks block matrix with 4-by-4 blocks.
            // Since we need to transpose the 'other' matrix, we will be moving along colums
            // so as to minimize the number of transpositions.
            for (int j = 0; j < nColumnBlocks; j++)
            {
                // First (l = 0) we have to initialize result matrix and then cover the remaining blocks.
                if (nAuxBlocks > 0) // Check if nAuxBlocks > 0, i.e. N > 3
                {
                    quad_row_transpose<4, 4>(
                        other.get_sse_element(0, j), other.get_sse_element(1, j), other.get_sse_element(2, j), other.get_sse_element(3, j),
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, 4, false>(
                                this->get_sse_element(4 * i, 0), this->get_sse_element(4 * i + 1, 0), this->get_sse_element(4 * i + 2, 0), this->get_sse_element(4 * i + 3, 0), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, j), result.get_sse_element(4 * i + 1, j), result.get_sse_element(4 * i + 2, j), result.get_sse_element(4 * i + 3, j),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, 4, false>(
                                this->get_sse_element(4 * nRowBlocks, 0), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, 0), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, 0), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, j), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, j), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, j), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }

                for (int l = 1; l < nAuxBlocks; l++)
                {
                    quad_row_transpose<4, 4>(
                        other.get_sse_element(4 * l, j), other.get_sse_element(4 * l + 1, j), other.get_sse_element(4 * l + 2, j), other.get_sse_element(4 * l + 3, j),
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, 4, true>(
                                this->get_sse_element(4 * i, l), this->get_sse_element(4 * i + 1, l), this->get_sse_element(4 * i + 2, l), this->get_sse_element(4 * i + 3, l), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, j), result.get_sse_element(4 * i + 1, j), result.get_sse_element(4 * i + 2, j), result.get_sse_element(4 * i + 3, j),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, 4, true>(
                                this->get_sse_element(4 * nRowBlocks, l), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, l), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, l), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, j), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, j), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, j), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }

                // Last auxiliary row
                if (N % 4 != 0)
                {
                    quad_row_transpose<N % 4, 4>(
                        other.get_sse_element(4 * nAuxBlocks, j), (N % 4) < 2 ? zero : other.get_sse_element(4 * nAuxBlocks + 1, j), (N % 4) < 3 ? zero : other.get_sse_element(4 * nAuxBlocks + 2, j), zero,
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, 4, nAuxBlocks != 0>(
                                this->get_sse_element(4 * i, nAuxBlocks), this->get_sse_element(4 * i + 1, nAuxBlocks), this->get_sse_element(4 * i + 2, nAuxBlocks), this->get_sse_element(4 * i + 3, nAuxBlocks), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, j), result.get_sse_element(4 * i + 1, j), result.get_sse_element(4 * i + 2, j), result.get_sse_element(4 * i + 3, j),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, 4, nAuxBlocks != 0>(
                                this->get_sse_element(4 * nRowBlocks, nAuxBlocks), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, nAuxBlocks), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, nAuxBlocks), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, j), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, j), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, j), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }
            }
        
            // Last column
            if (K % 4 != 0)
            {
                // First (l = 0) we have to initialize result matrix and then cover the remaining blocks.
                if (nAuxBlocks > 0) // Check if nAuxBlocks > 0, i.e. N > 3
                {
                    quad_row_transpose<4, K % 4>(
                        other.get_sse_element(0, nColumnBlocks), other.get_sse_element(1, nColumnBlocks), other.get_sse_element(2, nColumnBlocks), other.get_sse_element(3, nColumnBlocks),
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, K % 4, false>(
                                this->get_sse_element(4 * i, 0), this->get_sse_element(4 * i + 1, 0), this->get_sse_element(4 * i + 2, 0), this->get_sse_element(4 * i + 3, 0), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, nColumnBlocks), result.get_sse_element(4 * i + 1, nColumnBlocks), result.get_sse_element(4 * i + 2, nColumnBlocks), result.get_sse_element(4 * i + 3, nColumnBlocks),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, K % 4, false>(
                                this->get_sse_element(4 * nRowBlocks, 0), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, 0), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, 0), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, nColumnBlocks), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, nColumnBlocks), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, nColumnBlocks), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }

                for (int l = 1; l < nAuxBlocks; l++)
                {
                    quad_row_transpose<4, K % 4>(
                        other.get_sse_element(4 * l, nColumnBlocks), other.get_sse_element(4 * l + 1, nColumnBlocks), other.get_sse_element(4 * l + 2, nColumnBlocks), other.get_sse_element(4 * l + 3, nColumnBlocks),
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, K % 4, true>(
                                this->get_sse_element(4 * i, l), this->get_sse_element(4 * i + 1, l), this->get_sse_element(4 * i + 2, l), this->get_sse_element(4 * i + 3, l), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, nColumnBlocks), result.get_sse_element(4 * i + 1, nColumnBlocks), result.get_sse_element(4 * i + 2, nColumnBlocks), result.get_sse_element(4 * i + 3, nColumnBlocks),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, K % 4, true>(
                                this->get_sse_element(4 * nRowBlocks, l), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, l), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, l), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, nColumnBlocks), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, nColumnBlocks), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, nColumnBlocks), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }

                // Last auxiliary row
                if (N % 4 != 0)
                {
                    quad_row_transpose<N % 4, K % 4>(
                        other.get_sse_element(4 * nAuxBlocks, nColumnBlocks), (N % 4) < 2 ? zero : other.get_sse_element(4 * nAuxBlocks + 1, nColumnBlocks), (N % 4) < 3 ? zero : other.get_sse_element(4 * nAuxBlocks + 2, nColumnBlocks), zero,
                        col0, col1, col2, col3,
                        tmp0, tmp1, tmp2, tmp3, zero);

                    for (int i = 0; i < nRowBlocks; i++)
                    {
                        quad_multiply<4, K % 4, true>(
                                this->get_sse_element(4 * i, nAuxBlocks), this->get_sse_element(4 * i + 1, nAuxBlocks), this->get_sse_element(4 * i + 2, nAuxBlocks), this->get_sse_element(4 * i + 3, nAuxBlocks), 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * i, nColumnBlocks), result.get_sse_element(4 * i + 1, nColumnBlocks), result.get_sse_element(4 * i + 2, nColumnBlocks), result.get_sse_element(4 * i + 3, nColumnBlocks),
                                tmp0, tmp1, tmp2, tmp3, zero);
                    }

                    // Last row
                    if (M % 4 != 0)
                        quad_multiply<M % 4, K % 4, true>(
                                this->get_sse_element(4 * nRowBlocks, nAuxBlocks), (M % 4) < 2 ? zero : this->get_sse_element(4 * nRowBlocks + 1, nAuxBlocks), (M % 4) < 3 ? zero : this->get_sse_element(4 * nRowBlocks + 2, nAuxBlocks), zero, 
                                col0, col1, col2, col3, 
                                result.get_sse_element(4 * nRowBlocks, nColumnBlocks), (M % 4) < 2 ? zero : result.get_sse_element(4 * nRowBlocks + 1, nColumnBlocks), (M % 4) < 3 ? zero : result.get_sse_element(4 * nRowBlocks + 2, nColumnBlocks), zero,
                                tmp0, tmp1, tmp2, tmp3, zero);
                }
            }

            return result;
        }
    }
}

#endif // AFTERMATH_NUMERICS_VSSE_MATRIX_HPP_INCLUDED
